#pragma once
#include <iostream>
#include <algorithm>
#include <math.h>
#include <string>
#include <vector>
#include "path_points.h"


//--------------------TrajectoryPoint:轨迹点-----------------------//
class TrajectoryPoint
{
public:
    TrajectoryPoint();
    ~TrajectoryPoint() = default;

public:
    // 世界坐标系下的参数
    double x;      // x position
    double y;      // y position
    double z;      // z position
    double theta;  // yaw in rad
    double kappa;  // curvature曲率
    double dkappa; // curvature曲率导数
    double v;      // Tangential velocity
    double a;      // Tangential acceleration
    // Frenet坐标系下的属性
    double relative_time; // relative_time
    double d;             // lateral offset
    double d_d;           // lateral speed
    double d_dd;          // lateral acceleration
    double s;             // s position along spline
    double s_d;           // s speed
    double s_dd;          // s acceleration

    double s_ddd;
    double d_ddd;

public:

    void set_x(double x_);
    void set_y(double y_);
    void set_z(double z_);
    void set_theta(double theta_);
    void set_s(double s_);
    void set_kappa(double kappa_);
    void set_dkappa(double dkappa_);
    void set_v(double v_);
    void set_a(double a_);
    void set_relative_time(double relative_time_);

private:
    PathPoint pathpoint_;
};

class DiscretizedTrajectory : public std::vector<TrajectoryPoint>
{
public:
    DiscretizedTrajectory() = default;

    /**
     * Create a DiscretizedTrajectory based on protobuf message
     */
    virtual ~DiscretizedTrajectory() = default;
};
